#include "Start.h"
#include "myusart.h"
#include "PID.h"
#include "M_MotionSolution.h"
#include "FreeRTOS.h"
#include "queue.h"
#include <Kalman.h>
#include "JY901B.h"
#include "xm_motor.h"
#include "data_recieve.h"
using namespace std;
#define PITCH_ZERO 7395.0f
#define ValueLimit(val, min, max) val > max ? (max) : (val < min ? (min) : (val))
#define ABS(a) a > 0 ? (a) : (-a)
float kp = 1;
float kd = 0.3;
float degree_delta;
float degree;
extern osThreadId Pitch_TaskHandle;
extern osThreadId LEDTaskHandle;
extern osThreadId DebugTaskHandle;
extern osThreadId ManageTaskHandle;
extern osThreadId FireTaskHandle;

extern "C" void vTaskDelay(const TickType_t xTicksToDelay);

enum
{
    // 电机ID号
    PitchMotor_ID = 0,
    FireMotor_ID = 1,
    // A板对应数据
    A_PackSize = 2,
    A_pitch = 0,
    A_fire = 1,

};
JY901QUEUE_S pitchfifo;
extern JY901QUEUE_S jy901data;
// A板发来的数据队列
QueueHandle_t Board_QueueHandle;
// JY901B数据队列
QueueHandle_t JY901B_QueueHandle;
void Setup(void)
{

    // Kalman_Init();
    // while (BMI088_INIT())
    // {
    //     HAL_GPIO_TogglePin(LED_R_GPIO_Port, LED_R_Pin);
    //     HAL_Delay(10);
    // }
    // 创建JY901B队列
    // JY901B_QueueHandle = xQueueCreate(5, sizeof(JY901QUEUE_S));
    // 创建遥控队列

    Board_QueueHandle = xQueueCreate(10, sizeof(Can_RxData));
    // JY901_USART_Init();
    DATA_Recieve_Init();
    Serial.start_rx();
}

void ManageTask_Init(void)
{
    //   motor_pos_zero(1);
    //   motor_enable(1);
    //   motor_mode(1,1);
    //   //motor_pos_zero(1);

    //   motor_enable(1);
    //   motor_mode(1,1);
    //   motor_pow_value(1, 0.7, 3, 2, 0.5);
    //   motor_speed_value(1,5);
}
// 5ms 读取一下数据
float BoardA_Data[A_PackSize] = {0};
void ManageTask_Loop(void)
{

    // 读取A板数据
    xQueueReceive(Board_QueueHandle, &BoardA_Data, 0);
    // xQueueReceive(JY901B_QueueHandle, &pitchfifo, 0);
    //  开始计算pitch环
    xTaskNotifyGive(Pitch_TaskHandle);
    // 开始计算开火环
    xTaskNotifyGive(FireTaskHandle);
    // Serial.myprintf("%.2f,%.2f\n", FilterPitch, PitchTaskPara[1]);
}

void PitchTask_Init(void)
{
}
float Pitch_Kp = 0;
void PitchTask_Loop(void)
{
    if (ulTaskNotifyTake(pdTRUE, portMAX_DELAY))
    {

        // degree_delta = (float)Can_RxData[0]/1000.0f;
        // degree = degree + degree_delta;

        float Pitch_Auto_out = 0;
        uint8_t Yaw_Data[8] = {0};
        if (view_data.target_set)
        {
            Pitch_Auto_out = Pitch_Kp * view_data.Y_d;
//			view_data.X_d  = 100;
            Yaw_Data[0] |= (view_data.X_d >> 0)&0xFF;
            Yaw_Data[1] |= (view_data.X_d>>8) & 0xFF;
            CAN_Send_BoardA(Yaw_Data);
        }
        // if(degree > 0.4) degree  = 0.4 ;
        // else if(degree < -0.2) degree  = -0.2;

        // motor_pos_value(1, degree + Pitch_Auto_out);
		//角度环
        Pitch_Angle_PID.reference = PITCH_ZERO + BoardA_Data[A_pitch] + Pitch_Auto_out ;
        Pitch_Angle_PID.reference = ValueLimit(Pitch_Angle_PID.reference, 7200, 7800);//限幅
        Pitch_Angle_PID.feedback = motor_chassis[0].angle;//电机反馈角度
        Pitch_Angle_PID.PosPIDCalc(1000); // 8
//		Pitch_Angle_PID.output = 0;
//        Pitch_W_PID.reference = Pitch_Angle_PID.output > 0 ? (Pitch_Angle_PID.output * Pitch_Angle_PID.output) : (-Pitch_Angle_PID.output * Pitch_Angle_PID.output);
        //角速度环
		Pitch_W_PID.reference = Pitch_Angle_PID.output;
		Pitch_W_PID.feedback = kalmanFilter(&KFP_TS7, motor_chassis[0].speed);//卡尔曼对反馈速度滤波
        Pitch_W_PID.PosPIDCalc(0);
        CAN_Send_Pitch_cmd(Pitch_W_PID.output);
    }
}

void FireTask_Init(void)
{
}

void FireTask_Loop(void)
{
    if (ulTaskNotifyTake(pdTRUE, portMAX_DELAY))
    {
        // 最大8192
        if (BoardA_Data[A_fire] > 0)
        {
            Fire_LM_PID.reference = 5500;
            Fire_RM_PID.reference = -5500;
            view_data.fire = 0;
            HAL_GPIO_TogglePin(LED_R_GPIO_Port, LED_R_Pin);
        }
        else if (Can_RxData[1] == -100)
        {
            // motor_pos_zero(1);
            // motor_enable(1);
            // motor_mode(1,1);
            // //HAL_Delay(2000);
            // motor_pow_value(1, 1, 5, 2, 0.5);
            // motor_speed_value(1,5);

            // motor_enable(1);
            // motor_mode(1,1);
            // //HAL_Delay(2000);
            // motor_pow_value(1, 1, 5, 2, 0.5);
            // motor_speed_value(1,5);
            // //motor_pos_zero(1);
        }
        else if (BoardA_Data[A_fire] == 0)
        {
            Fire_LM_PID.reference = 0;
            Fire_RM_PID.reference = 0;
        }

        // Fire_LM_PID.reference = -BoardA_Data[A_fire];
        Fire_LM_PID.feedback = lpf_operator(&lpf_current_gyro[0], motor_chassis[1].speed_rpm, 0.005f);
        Fire_LM_PID.PosPIDCalc(0);
        // Fire_RM_PID.reference = BoardA_Data[A_fire];
        Fire_RM_PID.feedback = lpf_operator(&lpf_current_gyro[1], motor_chassis[2].speed_rpm, 0.005f);
        Fire_RM_PID.PosPIDCalc(0);

        CAN_Send_Fire_cmd(Fire_LM_PID.output, Fire_RM_PID.output);
        //		vTaskDelay(5);
    }
}

void DebugTask_Init(void)
{
}
void DebugTask_Loop(void)
{
    //	CAN_Send_cmd(0,0);
    //	Serial.myprintf("%.2f,%ld\n", motor_chassis[0].speed, motor_chassis[0].Angle_actual);
    // Serial.myprintf("%.2f,%.2f\n", FilterRoll, FilterPitch);
    //    Serial.myprintf("%.2f,%.2f,%.2f\n", Pitch_Angle_PID.reference, Pitch_Angle_PID.feedback, Pitch_Angle_PID.output);
    // Serial.myprintf("%.2f,%.2f,%.2f\n", Pitch_Pos_PID.reference, Pitch_Pos_PID.feedback, Pitch_Pos_PID.output);
    // if (xQueueReceive(Board_QueueHandle, aq, 0xEA60) == pdTRUE) // 一分钟未操作自动复位
    // {
    //     HAL_GPIO_TogglePin(LED_B_GPIO_Port, LED_B_Pin);
    //     Serial.myprintf("%f,%f\n", aq[0], aq[1]);
    // }
    // HAL_GPIO_TogglePin(LED_B_GPIO_Port, LED_B_Pin);

     Serial.myprintf("%f,%f\n", Pitch_Angle_PID.reference, Pitch_Angle_PID.feedback);
    //  Serial.myprintf("%f,%f\n", Fire_LM_PID.reference, Fire_LM_PID.feedback);
    //  Serial.myprintf("好的\n");
    //  Debug_USART.myprintf("%.2f,%.2f,%.2f\n",roll,pitch,yaw);
    //  Serial.myprintf("%s\n", Serial.rxdata);
    //  Serial.myprintf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",gyro.roll,gyro.pitch,gyro.yaw,accel.x,accel.y,accel.z);
    //  Serial.myprintf("%.2f,%.2f\n", gyro.roll, f);
    //	Debug_USART.myprintf("%.2f,%.2f\n",gyro.yaw,filterval);
    //	Debug_USART.myprintf("%.2f\n",gyro.yaw);
    //	Debug_USART.myprintf("%.2f,%.2f,%.2f\n",ist8310_data.meg_data.x,ist8310_data.meg_data.y,ist8310_data.meg_data.z);
    //  Debug_USART.myprintf("%.2f,%.2f,%.2f,%.2f\n", motor_chassis[0].speed,motor_chassis[1].speed,motor_chassis[2].speed,motor_chassis[3].speed);
    //      Debug_USART.myprintf("chan1=%d,chan2=%d,chan3=%d,chan4=%d,chan5=%d,chan6=%d,chan7=%d\n",rc.ch0,rc.ch1,rc.ch2,rc.ch3,rc.sw1,rc.sw2,rc.roll);
    //    seekfree_assistant_data_analysis();
    //    for (uint8_t i = 0; i < 8; i++)
    //    {
    //        if ((seekfree_assistant_parameter_update_flag[i]))
    //        {
    //            //			printf("ok\n");
    //            seekfree_assistant_parameter_update_flag[i] = 0;
    //            HAL_GPIO_TogglePin(LED_R_GPIO_Port, LED_R_Pin);

    //            // Pitch_Angle_PID.kp = seekfree_assistant_parameter[0];
    //            // Pitch_Angle_PID.ki = seekfree_assistant_parameter[1];
    //            // Pitch_Angle_PID.kd = seekfree_assistant_parameter[2];
    //            // Pitch_Angle_PID.reference = seekfree_assistant_parameter[3];
    //            // CAN_Send_Pitch_cmd(seekfree_assistant_parameter[0]);
    //            // Serial.myprintf("%.2f\n", seekfree_assistant_parameter[0]);
    //            // twoKp = seekfree_assistant_parameter[0];
    //            // twoKi = seekfree_assistant_parameter[1];
    //            // Serial.myprintf("kp=%f,ki=%f,kd=%f\n", Pitch_PID.kp, Pitch_PID.ki, Pitch_PID.kd);
    //        }
    //    }
}

void Receive_Callback()
{
    // Serial.ReceivePack_DMA(0x2C, 0x5B, 10);
}

#if USE_OS
#include <new>
void *operator new(size_t size) throw(std::bad_alloc)
{
    return pvPortMalloc(size);
}

void operator delete(void *pointer) throw()
{
    vPortFree(pointer);
}
#endif
